// ROS2 机器人虚拟控制手柄 - JavaScript控制逻辑 (升级版)
// 作者: AI Assistant
// 功能: 通过WebSocket连接rosbridge，实现虚拟摇杆控制机器人
// ROSLIB版本: 最新版本 (支持ROS1/ROS2)

class RobotController {
    constructor() {
        this.ros = null;
        this.cmdVelTopic = null;
        this.isConnected = false;
        this.speedBoostActive = false;
        this.maxLinearSpeed = 1.0;
        this.maxAngularSpeed = 2.0;
        this.speedMultiplier = 1.0;
        this.boostMultiplier = 2.0;
        
        // ROS版本和兼容性设置
        this.rosVersion = 'ROS2'; // 默认ROS2
        this.topicNames = {
            cmd_vel: '/cmd_vel',
            odom: '/odom',
            scan: '/scan'
        };
        
        // 消息发布统计
        this.messageStats = {
            published: 0,
            failed: 0,
            lastPublishTime: null
        };
        
        // 虚拟摇杆状态
        this.joysticks = {
            movement: {
                active: false,
                centerX: 0,
                centerY: 0,
                currentX: 0,
                currentY: 0,
                maxDistance: 75
            },
            rotation: {
                active: false,
                centerX: 0,
                centerY: 0,
                currentX: 0,
                currentY: 0,
                maxDistance: 75
            }
        };
        
        this.currentVelocity = {
            linear: 0,
            angular: 0
        };
        
        this.init();
    }
    
    init() {
        // 检查ROSLIB版本
        this.checkROSLIBVersion();
        
        this.setupEventListeners();
        this.initJoysticks();
        this.startVelocityPublisher();
        this.addLog('系统初始化完成 (升级版)', 'success');
    }
    
    // 检查ROSLIB版本和兼容性
    checkROSLIBVersion() {
        if (typeof ROSLIB === 'undefined') {
            this.addLog('❌ ROSLIB未加载', 'error');
            return false;
        }
        
        const version = ROSLIB.REVISION || 'Latest';
        this.addLog(`✅ ROSLIB版本: ${version}`, 'success');
        
        // 检查关键组件
        const requiredComponents = ['Ros', 'Topic', 'Message', 'Service'];
        const missingComponents = requiredComponents.filter(comp => !ROSLIB[comp]);
        
        if (missingComponents.length > 0) {
            this.addLog(`⚠️  缺少组件: ${missingComponents.join(', ')}`, 'warning');
        } else {
            this.addLog('✅ ROSLIB组件完整', 'success');
        }
        
        return true;
    }
    
    // 设置事件监听器
    setupEventListeners() {
        // 连接/断开连接按钮
        document.getElementById('connect-btn').addEventListener('click', () => this.connect());
        document.getElementById('disconnect-btn').addEventListener('click', () => this.disconnect());
        
        // 控制按钮
        document.getElementById('emergency-stop').addEventListener('click', () => this.emergencyStop());
        document.getElementById('speed-boost').addEventListener('click', () => this.toggleSpeedBoost());
        document.getElementById('reset-position').addEventListener('click', () => this.resetPosition());
        
        // 日志清空按钮
        document.getElementById('clear-log').addEventListener('click', () => this.clearLog());
        
        // 键盘控制
        document.addEventListener('keydown', (e) => this.handleKeyDown(e));
        document.addEventListener('keyup', (e) => this.handleKeyUp(e));
    }
    
    // 初始化虚拟摇杆
    initJoysticks() {
        this.setupJoystick('movement');
        this.setupJoystick('rotation');
    }
    
    setupJoystick(type) {
        const joystick = document.getElementById(`${type}-joystick`);
        const handle = document.getElementById(`${type}-handle`);
        const base = joystick.querySelector('.joystick-base');
        
        const baseRect = base.getBoundingClientRect();
        this.joysticks[type].centerX = baseRect.width / 2;
        this.joysticks[type].centerY = baseRect.height / 2;
        
        // 触摸事件
        handle.addEventListener('touchstart', (e) => this.startJoystick(e, type), { passive: false });
        handle.addEventListener('touchmove', (e) => this.moveJoystick(e, type), { passive: false });
        handle.addEventListener('touchend', (e) => this.endJoystick(e, type), { passive: false });
        
        // 鼠标事件
        handle.addEventListener('mousedown', (e) => this.startJoystick(e, type));
        document.addEventListener('mousemove', (e) => this.moveJoystick(e, type));
        document.addEventListener('mouseup', (e) => this.endJoystick(e, type));
        
        // 窗口大小变化时重新计算中心点
        window.addEventListener('resize', () => {
            const newRect = base.getBoundingClientRect();
            this.joysticks[type].centerX = newRect.width / 2;
            this.joysticks[type].centerY = newRect.height / 2;
        });
    }
    
    startJoystick(event, type) {
        event.preventDefault();
        this.joysticks[type].active = true;
        document.getElementById(`${type}-handle`).classList.add('active');
        
        if (event.type === 'mousedown') {
            event.target.style.cursor = 'grabbing';
        }
    }
    
    moveJoystick(event, type) {
        if (!this.joysticks[type].active) return;
        
        event.preventDefault();
        
        const joystick = document.getElementById(`${type}-joystick`);
        const handle = document.getElementById(`${type}-handle`);
        const base = joystick.querySelector('.joystick-base');
        const rect = base.getBoundingClientRect();
        
        let clientX, clientY;
        if (event.type === 'touchmove') {
            clientX = event.touches[0].clientX;
            clientY = event.touches[0].clientY;
        } else {
            clientX = event.clientX;
            clientY = event.clientY;
        }
        
        // 计算相对于摇杆中心的位置
        const deltaX = clientX - rect.left - this.joysticks[type].centerX;
        const deltaY = clientY - rect.top - this.joysticks[type].centerY;
        
        // 计算距离
        const distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
        const maxDistance = this.joysticks[type].maxDistance;
        
        // 限制在最大距离内
        let finalX = deltaX;
        let finalY = deltaY;
        
        if (distance > maxDistance) {
            finalX = (deltaX / distance) * maxDistance;
            finalY = (deltaY / distance) * maxDistance;
        }
        
        // 更新摇杆位置
        handle.style.transform = `translate(${finalX}px, ${finalY}px)`;
        
        // 保存当前位置
        this.joysticks[type].currentX = finalX;
        this.joysticks[type].currentY = finalY;
        
        // 计算标准化值 (-1 到 1)
        const normalizedX = finalX / maxDistance;
        const normalizedY = finalY / maxDistance;
        
        // 更新显示和速度
        this.updateJoystickDisplay(type, normalizedX, normalizedY);
        this.updateVelocity(type, normalizedX, normalizedY);
    }
    
    endJoystick(event, type) {
        if (!this.joysticks[type].active) return;
        
        this.joysticks[type].active = false;
        const handle = document.getElementById(`${type}-handle`);
        handle.classList.remove('active');
        handle.style.transform = 'translate(0px, 0px)';
        handle.style.cursor = 'pointer';
        
        // 重置位置和速度
        this.joysticks[type].currentX = 0;
        this.joysticks[type].currentY = 0;
        this.updateJoystickDisplay(type, 0, 0);
        this.updateVelocity(type, 0, 0);
    }
    
    updateJoystickDisplay(type, x, y) {
        if (type === 'movement') {
            document.getElementById('move-x').textContent = x.toFixed(2);
            document.getElementById('move-y').textContent = (-y).toFixed(2); // Y轴反转
        } else if (type === 'rotation') {
            document.getElementById('rotation-value').textContent = x.toFixed(2);
        }
    }
    
    updateVelocity(type, x, y) {
        if (type === 'movement') {
            // 左摇杆控制线性运动
            this.currentVelocity.linear = -y * this.maxLinearSpeed * this.speedMultiplier;
        } else if (type === 'rotation') {
            // 右摇杆控制角度运动
            this.currentVelocity.angular = -x * this.maxAngularSpeed * this.speedMultiplier;
        }
        
        // 更新速度显示
        document.getElementById('linear-speed').textContent = Math.abs(this.currentVelocity.linear).toFixed(2);
        document.getElementById('angular-speed').textContent = Math.abs(this.currentVelocity.angular).toFixed(2);
    }
    
    // 连接到机器人
    connect() {
        const ip = document.getElementById('robot-ip').value;
        const port = document.getElementById('robot-port').value;
        
        if (!ip || !port) {
            this.addLog('请输入有效的IP地址和端口', 'error');
            return;
        }
        
        const url = `ws://${ip}:${port}`;
        this.addLog(`正在连接到 ${url}...`, 'warning');
        
        this.ros = new ROSLIB.Ros({
            url: url
        });
        
        this.ros.on('connection', () => {
            this.onConnected();
        });
        
        this.ros.on('error', (error) => {
            this.onError(error);
        });
        
        this.ros.on('close', () => {
            this.onDisconnected();
        });
        
        // 设置超时
        setTimeout(() => {
            if (!this.isConnected) {
                this.addLog('连接超时，请检查网络和机器人状态', 'error');
            }
        }, 10000);
    }
    
    onConnected() {
        this.isConnected = true;
        this.addLog('成功连接到机器人！', 'success');
        
        // 更新UI
        document.getElementById('status-text').textContent = '已连接';
        document.getElementById('status-indicator').classList.add('connected');
        document.getElementById('connect-btn').disabled = true;
        document.getElementById('disconnect-btn').disabled = false;
        
        // 自动检测ROS环境和话题
        this.detectROSEnvironment();
    }
    
    // 检测ROS环境和可用话题
    detectROSEnvironment() {
        this.addLog('🔍 正在检测ROS环境...', 'info');
        
        // 获取话题列表
        this.ros.getTopics((topics) => {
            this.addLog(`📋 发现 ${topics.topics.length} 个话题`, 'success');
            
            // 检测ROS版本 (根据话题名称模式)
            const hasROS2Topics = topics.topics.some(topic => 
                topic.includes('_msgs/msg/') || topic.startsWith('/parameter_events')
            );
            this.rosVersion = hasROS2Topics ? 'ROS2' : 'ROS1';
            this.addLog(`🤖 检测到 ${this.rosVersion} 环境`, 'success');
            
            // 寻找cmd_vel话题
            this.findBestCmdVelTopic(topics.topics);
            
            // 获取节点列表
            this.ros.getNodes((nodes) => {
                this.addLog(`🎯 发现 ${nodes.length} 个活跃节点`, 'success');
                const robotNodes = nodes.filter(node => 
                    node.includes('robot') || node.includes('base') || 
                    node.includes('move') || node.includes('nav')
                );
                if (robotNodes.length > 0) {
                    this.addLog(`🚗 机器人相关节点: ${robotNodes.slice(0, 3).join(', ')}`, 'info');
                }
            }, (error) => {
                this.addLog(`⚠️  获取节点列表失败: ${error}`, 'warning');
            });
            
        }, (error) => {
            this.addLog(`❌ 获取话题列表失败: ${error}`, 'error');
            // 使用默认设置
            this.createDefaultCmdVelTopic();
        });
    }
    
    // 寻找最佳的cmd_vel话题
    findBestCmdVelTopic(availableTopics) {
        // 按优先级排序的候选话题名称
        const candidates = [
            '/cmd_vel',
            '/mobile_base/commands/velocity',
            '/base_controller/cmd_vel',
            '/robot/cmd_vel',
            '/turtle1/cmd_vel',
            '/cmd_vel_mux/input/navi'
        ];
        
        let bestTopic = null;
        for (const candidate of candidates) {
            if (availableTopics.includes(candidate)) {
                bestTopic = candidate;
                break;
            }
        }
        
        if (bestTopic) {
            this.topicNames.cmd_vel = bestTopic;
            this.addLog(`✅ 找到cmd_vel话题: ${bestTopic}`, 'success');
        } else {
            this.addLog('⚠️  未找到标准cmd_vel话题，使用默认名称', 'warning');
            // 显示可用的运动相关话题
            const motionTopics = availableTopics.filter(topic => 
                topic.includes('cmd') || topic.includes('vel') || topic.includes('twist')
            );
            if (motionTopics.length > 0) {
                this.addLog(`🚗 可用的运动话题: ${motionTopics.join(', ')}`, 'info');
            }
        }
        
        this.createCmdVelTopic();
    }
    
    // 创建cmd_vel话题
    createCmdVelTopic() {
        // 修正消息类型判断
        const messageType = this.rosVersion === 'ROS2' ? 'geometry_msgs/msg/Twist' : 'geometry_msgs/Twist';
        
        this.cmdVelTopic = new ROSLIB.Topic({
            ros: this.ros,
            name: this.topicNames.cmd_vel,
            messageType: messageType
        });
        
        this.addLog(`🎯 cmd_vel话题已创建: ${this.topicNames.cmd_vel}`, 'success');
        this.addLog(`📝 消息类型: ${messageType}`, 'info');
        this.addLog('🎮 可以开始控制机器人了！', 'success');
    }
    
    // 创建默认cmd_vel话题（当自动检测失败时）
    createDefaultCmdVelTopic() {
        this.addLog('🔄 使用默认配置创建cmd_vel话题', 'warning');
        this.createCmdVelTopic();
    }
    
    onError(error) {
        this.addLog(`连接错误: ${error}`, 'error');
        console.error('ROS连接错误:', error);
    }
    
    onDisconnected() {
        this.isConnected = false;
        this.addLog('与机器人的连接已断开', 'warning');
        
        // 更新UI
        document.getElementById('status-text').textContent = '未连接';
        document.getElementById('status-indicator').classList.remove('connected');
        document.getElementById('connect-btn').disabled = false;
        document.getElementById('disconnect-btn').disabled = true;
        
        // 停止机器人
        this.currentVelocity.linear = 0;
        this.currentVelocity.angular = 0;
    }
    
    disconnect() {
        if (this.ros) {
            this.emergencyStop();
            this.ros.close();
            this.addLog('主动断开连接', 'warning');
        }
    }
    
    // 发布速度命令 (增强版)
    publishVelocity() {
        if (!this.isConnected || !this.cmdVelTopic) {
            return;
        }
        
        // 创建Twist消息
        const twist = new ROSLIB.Message({
            linear: {
                x: this.currentVelocity.linear,
                y: 0.0,
                z: 0.0
            },
            angular: {
                x: 0.0,
                y: 0.0,
                z: this.currentVelocity.angular
            }
        });
        
        try {
            // 发布消息
            this.cmdVelTopic.publish(twist);
            
            // 更新统计
            this.messageStats.published++;
            this.messageStats.lastPublishTime = Date.now();
            
            // 调试信息（只在非零速度时显示，避免日志过多）
            if (this.currentVelocity.linear !== 0 || this.currentVelocity.angular !== 0) {
                console.debug(`📤 发布速度: 线性=${this.currentVelocity.linear.toFixed(3)}, 角度=${this.currentVelocity.angular.toFixed(3)}`);
            }
            
        } catch (error) {
            this.messageStats.failed++;
            this.addLog(`❌ 消息发布失败: ${error}`, 'error');
            console.error('速度消息发布错误:', error);
        }
    }
    
    // 获取发布统计信息
    getMessageStats() {
        return {
            ...this.messageStats,
            publishRate: this.messageStats.lastPublishTime ? 
                Math.round(1000 / (Date.now() - this.messageStats.lastPublishTime)) : 0
        };
    }
    
    // 启动速度发布器
    startVelocityPublisher() {
        setInterval(() => {
            this.publishVelocity();
        }, 50); // 20Hz发布频率
    }
    
    // 急停
    emergencyStop() {
        this.currentVelocity.linear = 0;
        this.currentVelocity.angular = 0;
        
        // 重置摇杆位置
        document.getElementById('movement-handle').style.transform = 'translate(0px, 0px)';
        document.getElementById('rotation-handle').style.transform = 'translate(0px, 0px)';
        
        // 更新显示
        this.updateJoystickDisplay('movement', 0, 0);
        this.updateJoystickDisplay('rotation', 0, 0);
        document.getElementById('linear-speed').textContent = '0.00';
        document.getElementById('angular-speed').textContent = '0.00';
        
        this.addLog('急停执行！机器人已停止', 'warning');
    }
    
    // 切换加速模式
    toggleSpeedBoost() {
        this.speedBoostActive = !this.speedBoostActive;
        const button = document.getElementById('speed-boost');
        
        if (this.speedBoostActive) {
            this.speedMultiplier = this.boostMultiplier;
            button.classList.add('active');
            this.addLog('加速模式已开启', 'success');
        } else {
            this.speedMultiplier = 1.0;
            button.classList.remove('active');
            this.addLog('加速模式已关闭', 'warning');
        }
    }
    
    // 重置位置
    resetPosition() {
        this.emergencyStop();
        this.speedBoostActive = false;
        this.speedMultiplier = 1.0;
        document.getElementById('speed-boost').classList.remove('active');
        this.addLog('已重置所有控制状态', 'success');
    }
    
    // 键盘控制
    handleKeyDown(event) {
        if (!this.isConnected) return;
        
        const key = event.key.toLowerCase();
        const speed = 0.5 * this.speedMultiplier;
        
        switch (key) {
            case 'w':
                this.currentVelocity.linear = this.maxLinearSpeed * this.speedMultiplier;
                break;
            case 's':
                this.currentVelocity.linear = -this.maxLinearSpeed * this.speedMultiplier;
                break;
            case 'a':
                this.currentVelocity.angular = this.maxAngularSpeed * this.speedMultiplier;
                break;
            case 'd':
                this.currentVelocity.angular = -this.maxAngularSpeed * this.speedMultiplier;
                break;
            case ' ': // 空格键急停
                event.preventDefault();
                this.emergencyStop();
                break;
            case 'shift': // Shift键加速
                if (!this.speedBoostActive) {
                    this.toggleSpeedBoost();
                }
                break;
        }
        
        // 更新速度显示
        document.getElementById('linear-speed').textContent = Math.abs(this.currentVelocity.linear).toFixed(2);
        document.getElementById('angular-speed').textContent = Math.abs(this.currentVelocity.angular).toFixed(2);
    }
    
    handleKeyUp(event) {
        if (!this.isConnected) return;
        
        const key = event.key.toLowerCase();
        
        switch (key) {
            case 'w':
            case 's':
                this.currentVelocity.linear = 0;
                break;
            case 'a':
            case 'd':
                this.currentVelocity.angular = 0;
                break;
        }
        
        // 更新速度显示
        document.getElementById('linear-speed').textContent = Math.abs(this.currentVelocity.linear).toFixed(2);
        document.getElementById('angular-speed').textContent = Math.abs(this.currentVelocity.angular).toFixed(2);
    }
    
    // 添加日志
    addLog(message, type = 'info') {
        const logContent = document.getElementById('log-content');
        const logEntry = document.createElement('div');
        logEntry.className = `log-entry ${type}`;
        
        const timestamp = new Date().toLocaleTimeString();
        logEntry.textContent = `[${timestamp}] ${message}`;
        
        logContent.appendChild(logEntry);
        logContent.scrollTop = logContent.scrollHeight;
        
        // 限制日志条数
        const entries = logContent.querySelectorAll('.log-entry');
        if (entries.length > 100) {
            entries[0].remove();
        }
    }
    
    // 清空日志
    clearLog() {
        const logContent = document.getElementById('log-content');
        logContent.innerHTML = '<div class="log-entry">日志已清空</div>';
    }
}

// 页面加载完成后初始化控制器
document.addEventListener('DOMContentLoaded', () => {
    const controller = new RobotController();
    
    // 添加使用说明
    controller.addLog('=== 使用说明 ===', 'info');
    controller.addLog('1. 输入机器人IP和端口，点击连接', 'info');
    controller.addLog('2. 左摇杆控制前进后退，右摇杆控制左右转向', 'info');
    controller.addLog('3. 键盘控制: W/S前后, A/D转向, 空格急停, Shift加速', 'info');
    controller.addLog('4. 急停按钮可立即停止机器人', 'info');
    controller.addLog('=== 准备就绪 ===', 'success');
    
    // 暴露到全局作用域用于调试
    window.robotController = controller;
});

// 防止页面刷新时意外关闭
window.addEventListener('beforeunload', (event) => {
    if (window.robotController && window.robotController.isConnected) {
        event.preventDefault();
        event.returnValue = '确定要离开吗？这将断开与机器人的连接。';
        return event.returnValue;
    }
}); 